import SimpleOpenNI.*;
SimpleOpenNI kinect;
float handDistance;
float handAngle;
boolean handsTrack = false;

void setup() {
  kinect = new SimpleOpenNI(this);
  kinect.setMirror(true);
  kinect.enableDepth();
  kinect.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);
  size(640, 480);  
  fill(255, 255, 0);
  stroke(0, 0, 255); 
  strokeWeight(3); 
  smooth();  
}
void draw() {
  kinect.update();  
  image(kinect.depthImage(), 0, 0);   
  IntVector userList = new IntVector(); 
  kinect.getUsers(userList);   
  if (userList.size() > 0) {  
    int userId = userList.get(0); 
    if ( kinect.isTrackingSkeleton(userId)) {  
      drawSkeleton(userId);
      println("success draw Skeleton");
      handAngle= this.getAngle(userId);
      println("success calculate angle");
    }       
  handDistance=getJointDistance(userId,SimpleOpenNI.SKEL_RIGHT_HAND, SimpleOpenNI.SKEL_LEFT_HAND);
  text(handDistance,width-50,30); 
  text(handAngle,width-50,80);
  println("handsTrackFlag = "+ handsTrackFlag);
  } 
}  
void drawSkeleton(int userId) {  
  stroke(0); 
  strokeWeight(5);  
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HAND, SimpleOpenNI.SKEL_RIGHT_HAND); 
  noStroke();   
  fill(255, 255, 0); 
  drawJoint(userId, SimpleOpenNI.SKEL_RIGHT_HAND);
  drawJoint(userId, SimpleOpenNI.SKEL_LEFT_HAND);
} 
PVector get_rightHandVector (int userId){
  PVector rightHand = new PVector();
  kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_HAND,rightHand);
  return rightHand;
}
PVector get_leftHandVector (int userId){
  PVector leftHand = new PVector();
  kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HAND,leftHand);
  return leftHand;
}
float getZPosition (int userId){
  PVector head = new PVector();
  kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_HEAD,head);
  return head.z;
}
float getAngle(int userId){
  PVector rightHand = new PVector();
  PVector leftHand = new PVector();
  kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HAND,leftHand);
  kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_HAND,rightHand);
  
  
  //PVector r =new PVector(rightHand.x,rightHand.y);
  //PVector l = new PVector(leftHand.x,leftHand.y);
  PVector mid =leftHand.cross(rightHand);
  println(leftHand);
  println(rightHand);
  println(mid);
  PVector horizon = new PVector(rightHand.x,0);
  float angleInRadians = PVector.angleBetween(mid,horizon);
  return(angleInRadians);
  //println(angleInRadians, 10,10);


}
float getJointDistance(int userId, int jointID1, int jointID2) {  
  PVector joint1 = new PVector();  
  PVector joint2 = new PVector();  
  kinect.getJointPositionSkeleton(userId, jointID1, joint1); 
  kinect.getJointPositionSkeleton(userId, jointID2, joint2);  
  return PVector.dist(joint1, joint2); } 

void drawJoint(int userId, int jointID) { 
  PVector joint = new PVector();  
  float confidence = kinect.getJointPositionSkeleton(userId, jointID, joint); 
  if (confidence < 0.5) {
    return;  
  }
  // convert the detected hand position 
  // to "projective" coordinates 
  // that will match the depth image
  PVector convertedJoint = new PVector(); 
  kinect.convertRealWorldToProjective(joint, convertedJoint);
  ellipse(convertedJoint.x, convertedJoint.y, 5, 5);
} 

// user-tracking callbacks!  
void onNewUser(int userId) {
  println("start pose detection");
  kinect.startPoseDetection("Psi", userId); 
}  

void onStartPose(String pose, int userId){
  println("Started pose for user"); 
  kinect.stopPoseDetection(userId); 
  kinect.requestCalibrationSkeleton(userId, true); 
}

void onEndCalibration(int userId, boolean successful) {  
  if (successful) {     
  println(" User calibrated !!!"); 
  kinect.startTrackingSkeleton(userId);  
  }
  else {
    println(" Failed to calibrate user !!!");
    kinect.startPoseDetection("Psi", userId);  
  } 
}

boolean updateHandRelease (int userId){
  

}
